#
# main.py
#

# fun stuff to use
import time
import random
import math
import sys

from RobotCanvas import ErdosCanvas
# here's the base class, hiding details...
from ThreadedClient import * 

# not sure that this needs to be a class, but...
class RobotController(ThreadedClient):
    UNIFORM = 1
    TRIANGLE = 2
    GAUSSIAN = 3
    ONROBOT = 2
    ONROBOTRANDOM = 3
    """
    A class defining the graphical display and the behavior of
    a Roomba-like robot (except that it has a range sensor!)
    """
    
    def __init__(self, robotType, mapname):
        """
        This constructor delegates things to the ThreadedClient class
        """
        ThreadedClient.__init__(self, robotType, mapname)
        
        
    def main(self):
        """
        This while loop is, in essence, the life of the robot.
        Each time through the loop, you will want to 
           * read the sensors
           * decide on what action to take (if any)
           * execute the current action
        Then, it all begins again!
        """
        
        
        # pause for a second
        time.sleep(1)
        
        # these constants represent different states in our state machine
        STOP = 0
        KBD = 1
        TURN = 2
        GO = 3
        WANDER = 4
        robotTask = KBD      # this is the starting state
        # other useful constants
        THRESH = 16.0        # radius of the roomba, in cm
        senseThresh = 16.0
        noise = 0.1         
        
        # an example of how to get the map here in the controller
        segList = self.getMap()
        counter = 0
        for s in segList:
            #print 'segment #', counter, 'is (', s.x1, ',', s.y1, ') - (', s.x2, ',', s.y2, ')'
            counter += 1
        
        rangeHeading = 0.0   # where to point our range finder
        
        oldgoal = (0,0)      # the old goal
        goal = (0,0)         # the current goal
        
        normalized = False   # are my particles normalized
        mcl = False
        xold = 50
        yold = 50
        throld = 0
        ctr = 0
        senseType = self.TRIANGLE
        placementType = self.ONROBOT
        moveType = self.UNIFORM
        samplingTime = 1
        moveError = .1
        INITIAL_PARTICLES = 10
        while True:          # run until you hit 'Q' in the window 
                             # or control-c at the command prompt         
            
            # SENSE
            #
            # x is the x coordinate of the robot
            # y is the y coordinate of the robot
            # thd is the robot's heading, theta, in degrees
            # bump is a list of two bump sensor readings: left and right
            [x,y,thd], bump = self.getData()
            #print 'x,y,thd:', x, y, thd
            # get vision data here...
            print bump
            # get the range to the walls at a particular heading
            d = self.getRange(rangeHeading)     # in degrees

            # thr is the robot's heading, theta, in radians
            thr = math.radians(thd)             # nice of python to have this function

            # get any keypresses and goal locations (mouse clicks when 'g' is pressed)
            kbd, goal = self.getWindowEvents()  # at start, kbd == '' and goal == (0,0)

            if goal != oldgoal: 
                # a new goal has been clicked - let's confirm this on the screen             
                print 'New goal at x =', goal[0], 'and y =', goal[1]
                self.showGoal( goal )           # displays the goal in green
                oldgoal = goal                  # reset the old goal
          
            # have we gotten to the goal?
            if robotTask != STOP and robotTask != KBD:
                [realx, realy, realthr] = self.getRealPose()
                distToGoal = math.sqrt( (realx-goal[0])**2 + (realy-goal[1])**2 )
                if distToGoal < THRESH:
                    print 'At goal!'
                    self.setVels(0,0)
                    robotTask = KBD

            
            # PLAN
                        #
            # This stops if a bump is encountered - a pretty basic plan
            if bump[0] == True or bump[1] == True:
                print 'BUMP!',
                print '  [Left bump sensor:',  bump[0], ']  ',
                print '  [Right bump sensor:', bump[1], ']  '
                robotTask = STOP
                         
            
            # ACT
            #
            # here, we decide how to set velocities,
            # based on our current state
            if robotTask == GO:
                self.setVels(40,0,noise)
                robotTask = KBD     # we've stopped, so let's return
                    
            if robotTask == STOP:  
                self.setVels(0,0)   # stop!
                robotTask = KBD     # we've stopped, so let's return
            # 
            # the KBD state is for user-control
            #
            
            
                
                
                
            if robotTask == KBD:
                # We allow keypresses to override or set our actions
                # 
                #if kbd == '': self.setVels(0,0)
                if kbd == 'i': self.setVels(50,0,noise)
                elif kbd == 'k': self.setVels(-50,0,noise)
                elif kbd == 'j': self.setVels(20,50,noise)
                elif kbd == 'l': self.setVels(20,-50,noise)
                elif kbd == 'P': 
                
                    # Create a bunch of particles starting at the position of the robot.
                    
                    #print ErdosCanvas.min_x_found, ErdosCanvas.min_y_found, ErdosCanvas.max_x_found, ErdosCanvas.max_y_found
                    if(placementType == self.UNIFORM):
                        particles = [ [random.uniform(ErdosCanvas.min_x_found, ErdosCanvas.max_x_found),
                                       random.uniform(ErdosCanvas.min_y_found, ErdosCanvas.max_y_found),
                                       random.uniform(0,math.pi*2),1.0,0] for i in range(INITIAL_PARTICLES)]
                    elif(placementType == self.ONROBOT):
                        particles = [ [x,y,thr,1.0,0] for i in range(INITIAL_PARTICLES) ]
                    #elif(placementType == self.ONROBOTRANDOM):
                    #    particles = [ [x,y,random.uniform(0,math.pi*2),1.0,0] for i in range(INITIAL_PARTICLES) ]
                    self.queue.put([self.gui.setParticles, particles])
                    #xold = x
                    #yold = y
                    #throld = thr
                
                
                elif kbd == '1':
                    moveType += 2
                    if moveType == self.GAUSSIAN:
                        print 'Changed movement type to Gaussian'
                    if moveType == self.UNIFORM + 4:
                        print 'Changed movement type to Uniform'
                        moveType = self.UNIFORM
                elif kbd == '2':
                    senseType += 1
                    if senseType == self.UNIFORM:
                        print 'Changed sense type to Uniform'
                    elif senseType == self.TRIANGLE:
                        print 'Changed sense type to Triangle'
                    elif senseType == self.GAUSSIAN:
                        print 'Changed sense type to GAUSSIAN'
                    elif senseType == self.GAUSSIAN+1:
                        print 'Changed sense type to Uniform'
                        senseType = self.UNIFORM

                elif kbd =='3':
                    placementType += 1
                    if placementType == self.ONROBOT:
                        print 'Changed placement type to On Robot'
                    elif placementType == self.ONROBOTRANDOM:
                        print 'Changed placement type to On Robot with random angle'
                    elif placementType == self.ONROBOTRANDOM+1:
                        print 'Changed placement type to Uniform distribution'
                        placementType = self.UNIFORM

                elif kbd =='5':
                    INITIAL_PARTICLES -= 5
                    if INITIAL_PARTICLES < 5:
                        print 'There can not be less than 5 initial particles'
                        INITIAL_PARTICLES = 5
                    else:
                        print 'The number of initial particles was reduced to ', INITIAL_PARTICLES

                elif kbd =='6':
                    INITIAL_PARTICLES += 5
                    print 'The number of initial particles was increased to ', INITIAL_PARTICLES
                    
                elif kbd == '7':
                    senseThresh -= .5
                    if senseThresh <= 0:
                        print 'Can\'t have a negative sensing threshold'
                        senseThresh = .5
                    else:
                        print 'Decreased the sensing threshhold (more particles will be considered probable: THRESH = ', senseThresh, ')'
                elif kbd == '8':
                    senseThresh +=.5 
                    print 'Increased the sensing threshhold (less particles will be considered probable: THRESH = ', senseThresh, ')'
                elif kbd =='9':
                    samplingTime += .1
                    print 'Decreased sampling time to every ', samplingTime, 'secs'
                elif kbd == '0':
                    samplingTime -= .1
                    if samplingTime < .05:
                        print 'Can\'t have sampling time below .05 secs'
                        samplingTime = .05
                    else:
                        print 'Increased sampling time to every ', samplingTime, 'secs'
                elif kbd == '-':
                    moveError -= .01
                    if moveError < 0:
                        print 'Can\'t have negative noise'
                        moveError = 0
                    else:
                        print 'Decreased the noise to ', moveError
                elif kbd == '+':
                    moveError += .01
                    if moveError > 1:
                        print 'Can\t have greater than 1 noise'
                        moveError = 1
                    else:
                        print 'Increased the noise to ', moveError
                    
                elif kbd == 'M':
                    xold, yold, throld = self.move(x, xold, y, yold, thr, throld, particles, moveType, noise)

                elif kbd == 'S': 
                    self.sense(particles, senseThresh, d, senseType, bump)
                    normalized = True
        
                    
                    
                elif kbd == 'R' and normalized == True: 
                    particles = self.resample(particles)
                    normalized = False
                    
                    
                elif kbd == 'G':
                    mcl = not mcl
                    lastTime = time.time()
                    print 'toggle MCL'
                    
                elif kbd == ' ': 
                    robotTask = KBD
                    self.setVels(0,0)   
                
            # special case to allow us to quit
            if kbd == 'Q': break              # 'Q' quits the program
            
            currentTime = time.time()
            if mcl == True and currentTime-lastTime > samplingTime: 
                if ctr==0:
                    xold, yold, throld = self.move(x, xold, y, yold, thr, throld, particles, moveType, noise)
                    ctr += 1
                    lastTime = currentTime
                #elif ctr == 1:
                    self.sense(particles, senseThresh, d, senseType, bump)
                    ctr +=1
                    lastTime = currentTime
                #elif ctr == 2:
                    particles = self.resample(particles)
                    lastTime = currentTime
                    ctr = 0
            
            # We pause to let the graphics catch up...
            time.sleep(0.025) # 1/40 of a second
               
            
        # this is outside the while loop...
        # shutdown the robot and gui, if needed
        self.shutdown()
    
    def move(self, x, xold, y, yold, thr, throld, particles, moveType, moveError):
        dx = x - xold
        dy = y - yold
        thrVel = math.atan2(dy, dx)
        dthr_1 = thrVel-throld
        dthr_2 = thr - thrVel
        dist = math.sqrt(dx**2 + dy**2)
        for particle in particles:
            if(moveType == self.UNIFORM):
                distOffset = random.uniform(-1*moveError * abs(dist), moveError * abs(dist))
                angle1Offset = random.uniform(-1*moveError/2 * abs(dthr_1)-dist/100, moveError/2 * abs(dthr_1)+dist/100)
                angle2Offset = random.uniform(-1*moveError/2 * abs(dthr_2), moveError/2 * abs(dthr_2))
            elif(moveType == self.GAUSSIAN):
                distOffset = random.gauss(0,moveError*abs(dist))
                angle1Offset = random.gauss(0,moveError/2*abs(dthr_1)+abs(dist)/100)
                angle2Offset = random.gauss(0,moveError/2 * abs(dthr_2))
            randist = dist + distOffset
            # Rotate theta 1 with error
            particle[2] += dthr_1 + angle1Offset
            # Move particle
            particle[0] += math.cos(particle[2]) * randist
            particle[1] += math.sin(particle[2]) * randist
            # Rotate the rest of the way
            particle[2] += dthr_2 + angle2Offset
            
        thrVel = math.atan2(dx,dy)
        
        self.queue.put([self.gui.setParticles, particles])
        
        xold = x
        yold = y
        throld = thr
        
        print 'Move'
        #print xold, yold, throld
        return xold, yold, throld
                
                
    def sense(self, particles, senseThresh, d, senseType, bump):
        pSum = 0.0
        for particle in particles:
            pRange = self.getRange(0, particle[0:3])
            #particle[4]=pRange
            delta = abs(pRange-d)
            #print pRange, delta
            #UNIFORM SENSING
            if(senseType == self.UNIFORM):
                if delta > 2*senseThresh:
                    particle[3] *= 0.05
            #otherwise we multiply by 1... done!
            #TRIANGLE SENSING
            elif(senseType == self.TRIANGLE):
                if delta > 4*senseThresh:
                    particle[3] *= .05
                else:
                    particle[3] *= 1-delta/(4*senseThresh) + .05
            elif(senseType == self.GAUSSIAN):
                if delta > 4*senseThresh:
                    particle[3] *= .05
                else:
                    particle[3] *= 1/(math.sqrt(2*math.pi)*senseThresh*2) * math.exp(-1*delta**2/(4*senseThresh**2))+.05
            #print particle[0], particle[1], particle[2], particle[3]

            # bump sensing probability
            tempRoomba = roombaSim(None)
            tempRoomba.simmap = self.r.simmap
            tempRoomba.gpos = RoombaOdometricPosition(*particle[0:3])
            #print tempRoomba.gpos.vals()
            _, _, sensors = tempRoomba.getUpdatedPose()
            #print sensors
            # If the bump sensors matchm up the probability
            if sensors == bump:
                particle[3] *= 2
            pSum += particle[3]

            
        totalProb = 0.0
        # normalize
        for particle in particles:
            particle[3] /= pSum
            totalProb += particle[3]
            #print "n:", particle[3], particle[4], abs(particle[4]-d) > 4*THRESH
        #print 'observed range:', d
        #print 'totalProb', totalProb, 'psum', pSum
        print 'Sense'
        
    def resample(self, particles):
        newParticles = []
        
        for particle in particles:
            #for i in range(int(math.floor(particle[3] * len(particles)))):
            if particle[3] > (1.0 / float(len(particles))):
                newParticles.append(particle[:])
            #p = particle[3] * len(particles) % 1
            
        while len(newParticles) < len(particles):
            rand = random.random()
            rindex = random.randint(0,len(particles)-1)
            if rand < particles[rindex][3] or rand < 0.05:
                newParticles.append(particles[rindex][:])
            # TODO: If needed, add more breadth support
            
        
        ''' 
        
        for particle in particles:
            if particle[3] >= 1.0/len(particles):
                newParticles.append(particle[:])
                newParticles.append(particle[:])
            if particle[3] < 1.0/len(particles):
                if random.uniform(0,1) > .5:
                    newParticles.append(particle[:])
        '''
        self.queue.put([self.gui.setParticles, newParticles])
        
        #print 'There are ', len(newParticles), ' particles'
        print 'Resample'    
        return newParticles
        

# this is python's way of detecting if you've started this file from
# the command line and, if so, instantiating the above class and
# starting the run() method...
#
if __name__ == '__main__':      # are we at the command line?
        
    # our pre-provided array of maps...
    MAPS = [ 'map0.txt', 
             'map1.txt',
             'map2rooms.txt',
             'mapColors.txt',
             'mapMaze.txt',
             'mapHallways.txt',
             'mapLibraComplex.txt',
             'mapSontagKitchen.txt' ]
             
    # give it a map name and a robot type...
    #R = RobotController(ThreadedClient.REAL_ROOMBA, MAPS[1])  # create R
    R = RobotController(ThreadedClient.SIM_ROOMBA, MAPS[0])  # create R
    R.run()                     # run R


    
